#include "ballistic_solver/ballistic_solver_node.hpp"
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include <thread>
#include <chrono>
#include <limits>

using namespace std::chrono_literals;
using namespace rclcpp;

namespace rm_aim
{
    // -------------------------- 构造函数实现 --------------------------
    BallisticSolverNode::BallisticSolverNode(const rclcpp::NodeOptions &options)
        : Node("ballistic_solver_node", options), can_outpost_shoot_(true) , mode_delay(0.0)  // 初始状态：可发射
    {

        // 读取参数
        declareAllParameters();

        // 初始化订阅者
        tracker_sub_ = this->create_subscription<aim_interfaces::msg::Target>(
            "/tracker/target", 
            rclcpp::SensorDataQoS(),
            std::bind(&BallisticSolverNode::trackerCallback, this, std::placeholders::_1));

        // 初始化发布者
        ballistic_pub_ = this->create_publisher<aim_interfaces::msg::BallisticCmd>(
            "/ballistic/cmd", 10);

        RCLCPP_INFO(this->get_logger(), "BallisticSolverNode started! "
                                        "Coordinate: X(forward), Y(left), Z(up)");
    }

    // -------------------------- 回调函数实现 --------------------------
    void BallisticSolverNode::trackerCallback(const aim_interfaces::msg::Target::SharedPtr msg)
    {
        tracker_data_ = *msg;
        // 跟踪有效、id非空
        if (msg->tracking && !msg->id.empty())
        {
            // 解析模式（从tracker id）
            current_mode_ = (tracker_data_.id == "outpost") ? "outpost" : (tracker_data_.id == "green_light") ? "green_light"
                                                                                                              : "normal";

            RCLCPP_DEBUG(this->get_logger(), "Parsed mode: %s (tracker id=%s, relative yaw=%.3f rad)",
                         current_mode_.c_str(), tracker_data_.id.c_str(), tracker_data_.yaw);
            process_ballistic();
        }
        else
        {
            RCLCPP_DEBUG(this->get_logger(), "Tracker invalid: tracking=%d, id=%s, yaw=%.3f",
                         msg->tracking, msg->id.c_str(), msg->yaw);
        }
    }
    rcl_interfaces::msg::SetParametersResult BallisticSolverNode::onParametersChanged(
        const std::vector<rclcpp::Parameter> &parameters)
    {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        result.reason = "Success";

        for (const auto &param : parameters)
        {
            RCLCPP_DEBUG(this->get_logger(), "Parameter updated: %s = %s",
                        param.get_name().c_str(), param.value_to_string().c_str());

            // Normal模式延迟参数
            if (param.get_name() == "delay.normal.fixed_delay")
                normal_fixed_delay_ = param.as_double();
            else if (param.get_name() == "delay.normal.prediction_weight")
                normal_prediction_weight_ = param.as_double();
            // Outpost模式延迟参数
            else if (param.get_name() == "delay.outpost.angle_delay_scale")
                outpost_angle_delay_scale_ = param.as_double();
            else if (param.get_name() == "delay.outpost.outpost_time_bias")
                outpost_time_bias_ = param.as_double();
            else if (param.get_name() == "delay.outpost.min_delay")
                outpost_min_delay_ = param.as_double();
            // GreenLight模式延迟参数
            else if (param.get_name() == "delay.green_light.fixed_delay")
                green_light_fixed_delay_ = param.as_double();
            else if (param.get_name() == "delay.green_light.flight_time_weight")
                green_light_flight_time_weight_ = param.as_double();
            // 全局/其他参数
            else if (param.get_name() == "global.system_delay")
                system_delay_ = param.as_double();
            else if (param.get_name() == "bullet_speed")
                bullet_speed_ = param.as_double();
            else if (param.get_name() == "air_resistance.k1")
                k1_ = param.as_double();
        }

        return result;
    }

    // -------------------------- 核心逻辑函数实现 --------------------------
    void BallisticSolverNode::process_ballistic()
    {

        // 1. 计算精确瞄准点（考虑飞行时间和预测）
        geometry_msgs::msg::Point precise_target_pos = calculatePreciseAimPoint(tracker_data_);

        // 3. 计算水平距离
        double horizontal_dist = ballistic_solver_.calcHorizontalDist(precise_target_pos);

        // 4. 解算弹道角度（Pitch + Yaw）
        double pitch_rad = ballistic_solver_.solvePitch(precise_target_pos, bullet_speed_, gravity_, k1_);
        double pitch_deg = pitch_rad * 180.0 / M_PI;
        double yaw_rad =  ballistic_solver_.solveYaw(precise_target_pos);
        double yaw_deg = yaw_rad * 180.0 / M_PI;

        // 5. 构造弹道指令消息
        auto cmd_msg = aim_interfaces::msg::BallisticCmd();
        cmd_msg.header = tracker_data_.header; // 与tracker时间同步
        cmd_msg.pitch = pitch_deg;
        cmd_msg.yaw = yaw_deg;
        cmd_msg.current_mode = current_mode_;
        cmd_msg.shoot_status = 0;

        // 6. 按模式执行射击逻辑
        if (current_mode_ == "normal")
        {
            cmd_msg.shoot_status = 1;
            ballistic_pub_->publish(cmd_msg);
            RCLCPP_DEBUG(this->get_logger(), "Normal mode shoot: Pitch=%.2f°, Yaw=%.2f°, Distance=%.2fm",
                         pitch_deg, yaw_deg, horizontal_dist);
        }
        else if (current_mode_ == "outpost")
        {
            // 检查是否可发射（标志位为true才允许触发）
            if (!can_outpost_shoot_.load()) return;

            // 获取飞行时间
            double flight_time = ballistic_solver_.calcFlightTime(horizontal_dist, bullet_speed_, pitch_rad, k1_);
            // 计算射击延时
            double delay_ms = (mode_delay - flight_time) * 1000 + outpost_time_bias_ * 1000;
            delay_ms = std::max(0.0, delay_ms);

            // 锁定标志位：置为false（防止后续消息触发）
            can_outpost_shoot_.store(false);  // store() 原子写入

            // 异步延时发布
            std::thread([this, cmd_msg, delay_ms, horizontal_dist]()
                        {
                rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int>(delay_ms)));
                // 检查节点是否还存活
                if (rclcpp::ok())
                {
                    auto shoot_msg = cmd_msg;
                    shoot_msg.shoot_status = 1;
                    ballistic_pub_->publish(shoot_msg);
                    RCLCPP_DEBUG(this->get_logger(), "Outpost mode shoot! Delay=%.0fms, Pitch=%.2f°, Yaw=%.2f°, Distance=%.2fm",
                    delay_ms, shoot_msg.pitch, shoot_msg.yaw, horizontal_dist);
                    // 射击流程结束, 允许下次射击
                    can_outpost_shoot_.store(true);
                } })
                .detach();
        }
        else if (current_mode_ == "green_light")
        {
            cmd_msg.shoot_status = 1;
            ballistic_pub_->publish(cmd_msg);
            RCLCPP_DEBUG(this->get_logger(), "Green light mode shoot: Pitch=%.2f°, Yaw=%.2f°, Distance=%.2fm",
                         pitch_deg, yaw_deg, horizontal_dist);
        }
    }
    // -------------------------- 辅助函数实现 --------------------------

    void BallisticSolverNode::declareAllParameters()
    {
        // d = 42.50 mm, m = 41 g
        // 首先计算空气阻力系数 K = 0.5 * Cd * ρ * A / m
        /*Cd = 0.47 是球体的阻力系数
        ρ = 1.169 kg/m³ 是空气密度（约20°C时）
        A = π * R² 是弹丸的迎风面积
        m = 0.041 kg 是弹丸质量*/
        // k1 = 0.47 * 1.169 * (2 * M_PI * 0.02125 * 0.02125) / 2 / 0.041;

        // -------------------------- 全局基础参数 --------------------------
        bullet_speed_ = this->declare_parameter("bullet_speed", 15.5); // 子弹初速（m/s）
        gravity_ = this->declare_parameter("gravity", 9.8);            // 重力加速度（m/s²）
        system_delay_ = this->declare_parameter(
            "global.system_delay", 0.005, // 全局系统延迟（s）：图像处理+通信延迟
            rcl_interfaces::msg::ParameterDescriptor().set__description("Global system delay (image processing + communication)"));

        // -------------------------- 空气阻力参数 --------------------------
        k1_ = this->declare_parameter(
            "air_resistance.k1", 0.47 * 1.169 * (2 * M_PI * 0.02125 * 0.02125) / 2 / 0.041,
            rcl_interfaces::msg::ParameterDescriptor().set__description("Air resistance coefficient (precomputed for 42.5mm bullet)"));

        // -------------------------- Normal模式延迟参数 --------------------------
        normal_fixed_delay_ = this->declare_parameter(
            "delay.normal.fixed_delay", 0.05,                                                                // Normal模式固定延迟（s）：补偿目标移动
            createFloatParamDesc("Fixed delay for normal mode (compensate target movement)", 0.0, 0.5, 0.01) // 范围：0~0.5s
        );
        normal_prediction_weight_ = this->declare_parameter(
            "delay.normal.prediction_weight", 1.0,                                                                      // 飞行时间权重（0~2）：调整预测激进程度
            createFloatParamDesc("Weight for flight time in normal mode (0=conservative, 2=aggressive)", 0.0, 2.0, 0.1) // 范围：0~2.0
        );
        // -------------------------- Outpost模式延迟参数 --------------------------
        outpost_angle_delay_scale_ = this->declare_parameter(
            "delay.outpost.angle_delay_scale", 1.0, // 角度延迟缩放系数（0.8~1.2）：微调角度→时间转换
            createFloatParamDesc("Scale factor for angle-based delay (adjust angle→time conversion)", 0.8, 1.2, 0.01));
        outpost_time_bias_ = this->declare_parameter(
            "delay.outpost.outpost_time_bias", -0.11, // 固定偏置（s）：补偿前哨站打击时间偏置
            createFloatParamDesc("Outpost_time_bias for flight time in outpost mode (adjust flight time→delay conversion)", -0.2, 0.2, 0.01));
        outpost_min_delay_ = this->declare_parameter(
            "delay.outpost.min_delay", 0.0, // 最小延迟（s）：避免负延迟
            createFloatParamDesc("Minimum delay for outpost mode (avoid negative delay)", 0.0, 0.5, 0.01));
        // -------------------------- GreenLight模式延迟参数 --------------------------
        green_light_fixed_delay_ = this->declare_parameter(
            "delay.green_light.fixed_delay", 0.03, // GreenLight模式固定延迟（s）：灯光响应延迟
            createFloatParamDesc("Fixed delay for green light mode (compensate light response)", 0.0, 0.3, 0.01));
        green_light_flight_time_weight_ = this->declare_parameter(
            "delay.green_light.flight_time_weight", 0.9, // 飞行时间权重（0.5~1.5）：适配灯光目标特性
            createFloatParamDesc("Weight for flight time in green light mode (0.5=conservative, 1.5=aggressive)", 0.5, 1.5, 0.01));

        // -------------------------- 注册动态参数回调 --------------------------
        params_callback_handle_ = this->add_on_set_parameters_callback(
            std::bind(&BallisticSolverNode::onParametersChanged, this, std::placeholders::_1));
    }

    geometry_msgs::msg::Point BallisticSolverNode::calculatePreciseAimPoint(const aim_interfaces::msg::Target &target)
    {
        // 1. 获取初始目标3D坐标(前x、左y、上z）
        geometry_msgs::msg::Point final_aim_point = tracker_data_.position;

        // 2. 计算「数据延迟」：detector采集 + armor_tracker跟踪 + 数据传输的总延迟
        auto current_time = this->now();
        auto time_diff = (current_time - target.header.stamp).seconds();
        // 处理时间同步误差（避免负延迟，最小设为0）
        double data_delay = std::max(0.0, time_diff);

        // 3. 基础延迟：数据延迟 + 全局系统延迟（节点内部处理延迟）
        double base_delay = data_delay + system_delay_;

        // 4. 模式专属延迟计算
        double flight_time_weight = 1.0;

        if (current_mode_ == "normal")
        {
            mode_delay = normal_fixed_delay_;
            flight_time_weight = normal_prediction_weight_;
        }
        else if (current_mode_ == "outpost")
        {
            double rotate_speed = fabs(target.v_yaw);
            if (rotate_speed > 1e-6)
            {
                // 前哨站模式的预测时间就是根据角速度得旋转到正面的时间，如果快了和慢了调节角度延迟缩放系数
                double angle_delay = (fabs(target.yaw) / rotate_speed) * outpost_angle_delay_scale_;
                mode_delay = std::max(angle_delay, outpost_min_delay_);
                final_aim_point = ballistic_solver_.predictArmorPos(target, mode_delay);
                return final_aim_point;
            }
        }
        else if (current_mode_ == "green_light")
        {
            mode_delay = green_light_fixed_delay_;
            flight_time_weight = green_light_flight_time_weight_;
        }

        // 5. 总预测延迟 = 基础延迟（数据+系统） + 模式专属延迟
        double total_delay = base_delay + mode_delay;

        // 6. 预测击打点
        final_aim_point = ballistic_solver_.calcPreciseAimPoint(target, total_delay, flight_time_weight, bullet_speed_, gravity_, k1_);

        return final_aim_point;
    }
        rcl_interfaces::msg::ParameterDescriptor BallisticSolverNode::createFloatParamDesc(
        const std::string &description,
        double min_val, double max_val, double step)
    {
        rcl_interfaces::msg::ParameterDescriptor desc;
        desc.description = description;
        desc.floating_point_range.resize(1);
        desc.floating_point_range[0].from_value = min_val;
        desc.floating_point_range[0].to_value = max_val;
        desc.floating_point_range[0].step = step;
        return desc;
    }
} // namespace rm_aim

// -------------------------- ROS 2 组件注册 --------------------------
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rm_aim::BallisticSolverNode)